/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/

#ifndef _PARTICLEMATH_H_
#define _PARTICLEMATH_H_

#include "linelib.h"

#ifndef PI
	#define PI	3.141592654
#endif

#define NUMBER_OF_MEASUREMENTS 10	// Number of virtual scan lines
#ifndef LASER_MAX_DIST2
	#define LASER_MAX_DIST2 (80.0*80.0) // The maximum distance of the laser measurement squared
#endif 



#define VMAP 0.1					// Map uncertainty
#define VANG 0.1					// Angle uncertainty
#define VR	 0.02					// Uncertainty of the range measurement
#define FALSE_PROB 0.4				// "Probability" of the false measurement

#define VMEAS ((VMAP+VANG+VR)*(VMAP+VANG+VR)) // Whole measurement uncertainty

typedef struct __poseparticle2{
	float x;	// X - coordinate of the particle
	float y;	// Y - coordinate of the particle
	float a;	// Heading
	float p;	// Probability of the particle 
	float lik;	// Likelihood of the particle
}TPoseParticle;

typedef struct _poseparticledistribution{
	TPoseParticle *Particles;	// Particle distribution
	float Lik;					// Likelihood of the distribution
	float outLiers;			// Percentage of the outlier measurements in distribution
	int NumOfParticles;			// Number of particles in distribution
	int size;					// reserved memory Size 
} TPoseParticleDistribution;


TPoseParticleDistribution newPoseParticleDistribution(int size);
void freePoseParticleDistribution(TPoseParticleDistribution PPD);

float uniformRandom(void);
float normalRandom();
void newRandomSeed();

TPose calculateMeanPosition(TPoseParticleDistribution PPD);

TPose calculateParticleVariances(TPoseParticleDistribution PPD);

TPoseParticleDistribution SIRupdate(TPoseParticleDistribution PPD);

TPoseParticleDistribution penaPredict(
									  TPoseParticleDistribution PPD,
									  float v[3],
									  float Q[3]);

TPoseParticleDistribution penaPredictCov(
										 TPoseParticleDistribution PPD,
										 float v[3],
										 float Q[4]);

TPoseParticleDistribution normalizeDistribution(TPoseParticleDistribution PPD);

TPoseParticleDistribution estimateInitialPosition( 
												  TPose initPose,
												  int size,
												  float v[3],
									   			  float Q[3],
									   			  TLineVector cMap,
												  float *scan);


void getScanLines(float *scanLines,TPoseParticle P, int number_of_measurements);

int findVirtualScan(float *Scan, 
					 float *scanLines, 
					 TLineVector map,
					 TPoseParticle Pos,
					 int number_of_measurements);

TLineVector findVisibilityLines( 
					 TLineVector map,
					 TPoseParticle Pos
					 );

TPoseParticleDistribution calculateLikelihood(
											  TPoseParticleDistribution PPD, 
											  TLineVector Map,
											  float *Scan);

TPoseParticleDistribution MCLBasicCovStep(
										  TPoseParticleDistribution PPD,
										  float v[3],
										  float Q[4],
										  TLineVector cMap,
										  float *scan);

TPoseParticleDistribution MCLBasicStep(
									   TPoseParticleDistribution PPD,
									   float v[3],
									   float Q[3],
									   TLineVector cMap,
									   float *scan);

TPoseParticleDistribution MCLBasicStepNoSIR(
									   TPoseParticleDistribution PPD,
									   float v[3],
									   float Q[3],
									   TLineVector cMap,
									   float *scan);


TPoseParticleDistribution ASIRStep(
									   TPoseParticleDistribution PPD,
									   float v[3],
									   float Q[3],
									   TLineVector cMap,
									   float *scan);



#endif
